/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file trajectoryserver.h
 *  功能:
 *　提供轨迹规划服务
 *
 *                                                                         *
 ***************************************************************************/
#ifndef TRAJECTORYSERVER_H
#define TRAJECTORYSERVER_H
#include <kdl/frames.hpp>
//#include "GeneralDefine.h"
#include "axisgroup.h"
//#include "axisgroup.h"
#include "trajectoryudefine.h"
#include "coordinatemanager.h"
#include "MacroDefine/GeneralDefine.h"
#include "motionmessage.h"
#include "dragcontrol.h"
#include "ecattype.h"
#include "vehiclejogcontrol.h"
#include "virtuallinecontrol.h"
#include "vehiclepositionmanage.h"

#define D_VEHICLE_CONFIG_FILE "/"

struct HomeFinishCommand
{
    bool isOneAxis;
    bool isFinished;
    bool isCalibrated;
    bool isExit;
    int axisId;
};

class WaveMotion;
using namespace KDL;
class RosApi;
class Cu1SensorAdapter;
class QrcodeSensorAdapter;
class MagneticTrack;
class BeltTrackManager;
class RegData;
class RobotIo;
class CoupleControlManagement;
class GantryControl;
class ZeroCalculate;
class CarvingModule;
//class VehicleJogControl;

/**
 * @brief The TrajectoryServer class 运动控制服务器
 */
class TrajectoryServer
{
public:
    TrajectoryServer(RobotCommonParameter *roboParameterIn,
                     MotionMessage * motionMessageIn, RosApi *rosApiIn,RegData *regDataIn, RobotIo *ioControllerIn);
    ~TrajectoryServer();

public:
    int getEmcDebugInfo(EmcDebugInfo &debugOut);
    int emcSetVelLimit(double maxLineVelIn);
    int isInCarvingMode();
    int setIsInCarvingMode(bool isIn);
    int setCollisionWarnTorque(int axisIndex,double  torqueIn);
    int setCollisionDetectRatio(double ratioIn);
    int getIdealTorque(QVector<double>& torqueOut);
    int setIdealTorque(QVector<double> torqueIn);
    int setGravityVector(vector<double> gravityIn);
    int setToolMass(int toolIndex, double mass, vector<double> center);
    int setSegmentAttribute(int index,JointAttribute attributeIn);
    int setDragPara(int axisIndex,DragPara paraIn);
    int getTrigPosition(QQueue<PointPro> &trigPoseOut);
    int stopMotionTrig(int doIndex);//结束触发
    int setMotionTrigPara(int doIndex,double trigPeriod);//开始触发

    int setRobotCoordinateConfig(struct RobotCoordinateAxisConfig coordinateConfigIn);
    double getAxisGroupStopTime();
    double getAxisGroupVelLimitRatio();
    int initialVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn);
    int getVehicleMovingCaligrateInfo(struct VehicleMovingCaligrateInfo &infoOut);
    int setCalibrateWheelDiameter(double idealMovedLength,double actualMovedLength);
    int setCalibrateWheelDistance(double idealMovedAngle,double actualMovedAngle);

    int getEncoderValue_belt(int beltIndexIn,int &encoderValueOut);
    int getEncoderValue_vision(int visionIndexIn,int &encoderValueOut);
    int forceConnectCamera(int visionIndex);
    int forceTrigCamera(int visionIndexIn,bool isHighIn);
    int getImuDeviceData(ImuDataInfo &dataOut);
    int setCoupleRelativeDistanceMonitorEnable(int followAxisId,bool isEnableIn);
    int getCoupleControlInfo(std::vector<struct CoupleConfigParameter> &infoOut);
    int setRotateEqradius(double rotate_eqradiusIn);
    int setAxisProperty(int axisId, struct AxisProperty_hmi axisPropertyIn);
    int setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn,
                                                      double approachDistanceIn);
    int getVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter &infoOut);
    int setVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter infoIn,int &returnFlag);
    int getAgvBorderInfo(struct VehicleBorderInfo &infoOut);
    int setAgvBorderRectInfo(struct BorderRectInfo rectInfoIn,int &returnFlag);

    int getVehicleKinematicDimension(struct VehicleKinematicDimension &infoOut);
    int setVehicleKinematicDimension(struct VehicleKinematicDimension infoIn,int &returnFlag);
    int getVehicleJogControlParameter(struct VehicleJogControlParameter &infoOut);
    int setVehicleJogControlParameter(struct VehicleJogControlParameter infoIn,int &returnFlag);
    int getAgvSheftInfo(int sheftIndex, struct BorderRectInfo &infoOut);
    int setAgvSheftRectInfo(int sheftIndex,struct BorderRectInfo rectInfoIn,int &returnFlag);
    int getVehiclePositionCalibrateInfo(struct VehiclePosition &infoOut);

    int setCameraPointCoordinate(double x,double y,double z);
    int getCameraPointInBase(double &x,double &y,double &z);

    int getWeldPositionByCameraTransfer(const PointPro tcpPosition,double x,double y,double z,
                                        PointPro &pointOut);
    int calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList, LaserCameraCalibrateResult &resultOut);
    int getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut);
    int setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn);
    int setRobotArmLineMaxAcceleration(double accelerationIn);
    int setRobotArmLineMaxVelocity(double velIn);
    int setRobotArmJointMaxAcceleration(int axisIdIn,double accelerationIn);
    int setRobotArmJointMaxVelocity(int axisIdIn, double velIn);
    int activateCameraCoordinate(int index);
    int getCurrentCameraIndex();
    int pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                               const PointPro cameraPointIn,
                                               PointPro &worldPointOut);
    int calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList,
                                  std::vector<float> &cameraTransferOut, double &maxErrorOut);
    int getVisionTrackConfigList(std::vector<struct VisionTrackConfig> &configListOut);
    int getBeltTrackConfigList(std::vector<struct BeltTrackConfig> &configListOut);
    int calculateZeroOffset(std::vector<PointPro> pointList,std::vector<double> &zeroOffsetOut
                            ,double &maxErrorOut);
    int controlTypeSwitch(int controlTypeIn);
    int setWaveAcceleration(double accelerationIn);
    int startWave();
    int stopWave();
    int setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn,double wavePeriodIn);
    int setVisionTrigObjectLength(int visionIndex,double lengthIn);
    int setTrackOffset(int beltIndex, double offset_x, double offset_y);
    int getMotorEncoderPulsePerRound(QVector<double>& pulseOut);
    int setEncoderReceiverOutput(unsigned short doIn);
    int setSteeringControlKp(double kpIn, double maxSteerAdjustIn);
    int setVehicleJogDebugFlag(int flagIn);
    int getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut, int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut,
                           double &currentPathParameter1Out, double &currentPathParameter2Out);
    int resetCoupleOffset();//not used
    int getCoupleConfigInfo(int followAxisId,CoupleConfigParameter &coupleParaOut);
    int setCoupleControlEnable(int coupleId,bool isEnableIn);
    int setCoupleControlOffset(int coupleId,double offfsetValueIn);
    int getVisionTrackDebugInfo(int visionIndex,VisionTrackDebugInfo &visionInfoOut);
    int getBeltTrackDebugInfo(int beltIndex,BeltTrackDebugInfo &beltTrackInfoOut);
    int modifyVisionTrackConfig(int visionIndex, VisionTrackConfig configIn);
    int modifyBeltTrackConfig(int beltIndex,BeltTrackConfig configIn);
    int runVisionTrack(int visionIndex);
    int trigVisionOnce(int visionIndex);
    int getVisionData(int visionIndex);
    int clearVisionData(int visionIndex);
    int getTrackData(int beltIndex);
    int beltTrackStart(int beltIndex);
    int beltTrackEnd(int beltIndex);

    int getEncoderReceiverData(std::vector<EncoderReceiverData> &dataListReturn);
    E_BARRIER_RUN_MODE getBarrierRunMode();
    int setBarrierRunMode(E_BARRIER_RUN_MODE modeIn);
    int getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut);
    int setQrcodeDetectParameter(QrcodeDetectParameter parameterIn);
    int setQrcodeCameraId(int cameraIdIn);
    int showQrcodeDebugWindow(int id);
    int getQrcodeSensorDebugInfo(QrcodeDebugInfo &debugInfoOut);
    int getCu1SensorRawData(int &requestCountOut, double &l1Out, double &l2Out, double &imageOffsetOut,
                            bool &isSucessOut, int &errorCodeOut);
    int setDebugFlag(int flagIn);
    int setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn);
    int getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut);
    int getSafeCollisionInfo(int sensorType, CollisionPara &infoOut);
    int setSafeCollisionInfo(int sensorType, CollisionPara infoIn,int &returnFlag);
    int getMotorControlMode(QVector<E_MOTOR_CONTROL_MODE> &controlModeOut);
    int getMotorControlMode(int axisId,E_MOTOR_CONTROL_MODE &modeOut);
    int setCurrentVehicleBorderId(int idIn);
    int getCurrentVehicleBorderId();
    int isVirtualLineTrajectoryEmpty();
    int setVirtualLineEnableKpChange(bool isEnable);
    int clearAllMsg( );
    int setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn,bool isCheck);
    int setCheckVehiclePosition_amcl(bool isCheck);
    int setCheckVehiclePosition_vmark(bool isCheck);
    int setCheckVehiclePosition_qrcode(bool isCheck);
    int getVehicleSafeSpeedRatio(double &ratioOut);
    int setAgvTargetAllowOffset(float allowLineOffsetIn,float allowAngleOffsetIn);
    int changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn);
    int getTimeConsumed_pointCloudDetect(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                          double kpOnlyRotationIn, double kpDistanceError_yIn);
    int getTargetCommandVehiclePosition(VehiclePosition &targetOut, int &pointIndexOut);
    int getCarBorder(std::vector<VehiclePosition> &carBorderOut);
    int getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut);
    int setVehicleBorderOffset(double front_offsetIn,double back_offsetIn,double left_offsetIn,double right_offsetIn);
    int setUltrasonicSensorCheckEnable(bool isOn);
    int getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut,bool &isCheckEnableOut);
    int getPointCloudSafeStatus(std::vector<std::string > &sensorNameOut,std::vector<double> &sensorSafeRatioOut,
                                bool &isCheckEnableOut);
    int setPointCloudCheckEnable(bool isOn);
    int calibrateVehicle(double xOffset,double yOffset,double rOffset);
    int getVehicleSpeedFeedback(VehicleVelocity &velocityOut);
    int resetCustomOdometer_positionManage();
    int getOdometer_positionManage(double &historyOdomOut, double &historyTimeOut, double &customOdomOut, double &customeTimeOut);
    int controllerShutDown();
    int setVehicleAcceleration(double accelerationIn);
    int setVehicleAcceleration_rotate(double accelerationIn);
    int getVehicleFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut);
    bool isInitialOk();

    int getTrackPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput);
    int addVirtualLineTrajectoryCommand(QVector<NavParameter> commandVectorIn);//添加一条虚拟轨迹轨迹命令
    int setMaganeticInputIndex(int indexIn);

    int writeTagData(int aliasIn, long long cardValueIn);
    int writeTagData(int aliasIn, QVector<unsigned char> cardValueIn);
    int getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected);
    int getCurrentTagData(int aliasIn, QVector<unsigned char> &cardValueOut, bool &isCardDetected);
    int getMaganeticSensorRawValue(int aliasIn,QVector<bool> &sensorRawValueOut);
    int getVehiclePosition_specified(PositionInfo& positionReturn,E_VEHICLE_POSITION_TYPE typeIn);
    int getVehiclePosition(struct VehiclePosition& positionReturn);
    int getVehiclePosition(struct VehiclePosition& positionReturn,E_VEHICLE_POSITION_TYPE typeIn);
    int getVehicleVelCommand(bool isJogCommand, double &xMove, double &yMove, double &rotate);
    //------------agv
    int startGoZero_forUser(int axisId);
    int startGoZero_forUser();
    int haltGoZero_forUser();

    bool isAbsoluteMotor(int axisId);

    int changeToHomingMode(int axisId);
    int changeToHomingMode();
    int startGoZero(int axisId);
    int haltGoZero();
    int getZeroMovingStatus(int axisId, EM_ZERO_MOVING_STATUS &statusOut);
    int restorNormalOperationMode(int axisId);
    int restorNormalOperationMode();

    int algorithmFunctioncall(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                              QVector<MotorData> &motorCommandOut, int &returnMutexFlag, QVector<E_MOTOR_CONTROL_MODE> &controlModeOut);//算法循环调用函数入口。
    int getDebugInfo(MotionDebugInfo& infoOut);
    int getVirtuleLineDebugInfo(VirtualLineDebugInfo& infoOut);
    int setMoveToPointFlag();
    int getWheelData(vector<double>& wheelMoveSpeed,vector<double>& wheelPathLength);
    int getOdometer(VehicleVelocity& vehicleVelFeedbackOut,
                    VehiclePosition& vehiclePositionFeedbackOut);
    int vehicleAuto(double xMove,double yMove,double rotate);
    int vehicleJog(double xMove, double yMove, double rotate);
    int startVehicleJogMove();
    int stopVehicleJogMove();
    int setVehicleMagneticMovePid(int idOfPid, TrackPidInfo pidInfo);

//    int getMaganeticDi(QVector<bool>& diOut);
    int resetVehicleMagneticMove();
    int startVehicleMagneticMove();
    int setMageneticPassRfid(long long passRfidIn);
    int getTrackStatus(E_TRACK_STATUS &statusOut);
    int setTrackMode(E_TRACK_MODE modeIn);
    int setTrackStopCondition(E_TRACK_STOP_CONDITION conditionIn);
    int setVehicleIdealSpeed_ratio(double speedIn);
    int setVehicleIdealSpeed_mPerS(double speedIn);// m/s
    int setMaganeticMovingSpeedRatio(double speedRatioIn);
    int setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn);
    int moveOneStepByVelocity(int axisId, double velocityIn);//如果非实时线程调用，会阻塞ethercat线程
    int moveOneStepByVelocity(QVector<double> velocity);//如果非实时线程调用，会阻塞ethercat线程
    int moveOneStepByPV(double positionIn, double velIn ,int axisId);//如果非实时线程调用，会阻塞ethercat线程
    int setTrajectoryContinuousRun(bool runMethod);
    int setMotionServerModel(MC_MOTION_MODEL modelIn);
    int motionServerLoopRealTime();
    int motionServerLoopCommon();
    int trajectoryProcessLoopRealTime();
    int trajectoryProcessLoopCommon();
    int velocitySlowdownLoopRealtime(bool isTpValid=false);
    int beltTrackLoop();

    int setBasicStatus(MC_TRAJECTORY_STATUS statusIn,int comment=0);
    int setCheckMode(E_CHECK_MODE modeIn);

    int servoOn();//开所有必须轴.有个时间差，原点标定完成，到反馈位置真正生效。导致猛冲到０
    int servoOn_all();//开所有轴
    int servoOff_all();//关所有
    bool isServoOn();//必须轴是否全部开启
    bool isServoOn_all();//所有轴
    bool isMustOnAxis(E_AXIS_LOGIC_TYPE axisLogicTypeIn);
    int servoOn(int axisId);//实时线程和非实时线程同时操作，可能导致coredunmp　//速度是０时，才能使能
    int servoOff(int axisId);//实时线程和非实时线程同时操作，可能导致coredunmp
    bool isServoOn(int axisId);//实时线程和非实时线程同时操作，可能导致coredunmp
    E_ECAT_MASTER_STATE getMasterState();
    int quickStop();
    int initialServoParameter(QVector<JointAttribute>  jointAttributeVectorIn);
    int initialServoParameter();
    //hualei ,need todo , cost to much time,sometime more than 1.2ms.
    int setMotorFeedbackDataVectorFromEthercat_UpdateCurrentJointPointCoordinatePoint(const QVector<MotorData> &motorFeedback);
    int setMotorFeedbackDataVectorFromEthercatNoLock();
    double getDeltaTime();
    int allowSingleAxisJog(bool enable);
    int trajectoryRun();
    int startOneModel(MC_MOTION_MODEL modelIn);
    int allStartRun();
    int virtualLineRun();

    /**
     * @brief cease 暂无功能
     * @return
     */
    int allCease();
    int virtualLineCease();
    /**
     * @brief stop　停止
     * @return
     */
    int allStop();
    /**
     * @brief reset 清除历史轨迹数据、错误状态、从新加载axis命令值
     * @return
     */
    int resetAll(int id=0);//清除全部
    int resetAll_exceptStatus();
    int resetAll_and_virtualLine();
    int resetAll_and_virtualLine_no_clear_queue();
//    int stoppedStatusUpdate();
     int clearTraj(bool isSyncFeedbackPos);//清除全部，不包括时间轴
     int clearTraj_byAxisId(int axisIndex,bool isSyncFeedbackPos);
    int getStatus();
    int getCurrentJointPoint_Feedback(PointPro &jointPoint);
    int getCurrentCoordinatePoint_Feedback(PointPro &coordinatePoint);
    int getCurrentCoordinatePoint_Command(PointPro &coordinatePoint);
    int calculateCurrentPoint();//计算更新CurrentJointPoint_Feedback　CurrentCoordinatePoint_Feedback

    int setPositionFrameCommand_byJointCommand();
    int synchronizeMotorFeedbackToCalculatePosition();
    int synchronizeMotorFeedbackToCalculatePosition(int axisIndex);
    int syncAxisPoseCommandToCarvingModule();
    int addStopTraJectory();
    int addTrajectoryCommand(QVector<MoveParameter> tmpCommandVector);//添加一条轨迹命令
    int calculateTrajectoryPath();//根据轨迹命令队列，计算轨迹。需要周期循环调用。
    int calculateTrajectoryPath_cnt();
    int processQueueForCarvingModule();//not use
    int transCommandQueueToCarvingModule(QVector<MoveParameter>& tmpCommandVectorIn);
    int calculateOnePath(MoveParameter moveParameter, MoveParameter moveParameter2);
    int jointInstructionCalculate(QVector<MoveParameter> &moveParameterArry,
                                  MoveParameter moveParameter, int& queueGetedFlag);
    int updateTrajTime();
    int updateTrajTime_old();
    int addTrajTime(int lineNumber, double timeDuration);
    int resetTrajTime();
    int moveOneStep();
    int moveOneStep_carvingTp();
    int moveOneStepByTorque(double torqueIn,int axisId);
    int moveOneStepByPosition_plus(double positionPlus,int axisId);
    int timeAxisMoveOneStep();
    int axisGroupMoveOneStep();
    int calculatePointByTrajectory(KDL::Frame &frameReturn,int &lineNumber,double &timeRemainning);//通过轨迹计算点数据
    int writeDataToFile();
    int waveMoveOneStep();//摆焊接



//    int getCalibrateMotorValueOffset(QVector<int> &pulseValue);
    QVector<MotorData> getMotorCommandDataVector();
    QVector<MotorData> getMotorFeedbackDataVector();
    int setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn);
//    int setFrameFeedback();
    int setControlSpeedRatio(double controlSpeedRatioIn, double time);
    int setSafeSpeedRatio(double safeSpeedRatioIn, double time);
    int setAdjustSpeedRatio(double ratioIn, double time);
    int getTimeAxisStatus();
    int getTimeAxisStatus(int timeAxisIndex);
    int isTrajectoryEmpty();
    int isTrajectoryEmpty_nolock();
    int isTargetOffsetCheckOk();
    /**
     * @brief getToolCoordinateInfo
     * @return
     */
    QVector<ToolCoordinateInfo> getToolCoordinateList();
    /**
     * @brief getUserCoordinateList
     * @return
     */
    QVector<UserCoordinateInfo> getUserCoordinateList();

    /**
     * @brief getToolIndex
     * @return
     */
    int getToolIndex();
    /**
     * @brief getUserIndex
     * @return
     */
    int getUserIndex();

    /**
     * @brief getJointNum　所有轴
     * @return
     */
    int getJointNum();
    /**
     * @brief getJointMaxPosition 所有轴，不分内部轴，外部轴
     * @return
     */
    float getJointMaxPosition(int jointIndex);
    /**
     * @brief getJointMinPosition　所有轴，不分内部轴，外部轴
     * @return
     */
    float getJointMinPosition(int jointIndex);
    /**
     * @brief getJointType
     * @return
     */
    int getJointType(int jointIndex);
    int getAxisLogicalType(int jointIndex);
    int getAxisInformation(std::vector<struct AxisLimit> &axisLimit,std::vector<struct AxisLimit> &extAxisLimit,
                           std::vector<int> &axisUnit,std::vector<int> &extAxisUnit,std::vector<int> &axisType,
                           std::vector<int> &extAxisType);
    /**
     * @brief getSoftLimit                  获取软限位数据
     * @param axisLimit                     软限位数据
     * @param axisUnit                      软限位单位
     */
    void getSoftLimit(std::vector<std::vector <float> >  &axisLimit, std::vector<int> &axisUnit);

    int activateToolCoordinate(int index);
    int activateUserCoordinate(int index);
    int setToolCoordinate(ToolCoordinateInfo& toolCoordinate);
    int setUserCoordinate(UserCoordinateInfo& userCoordinate);

    /**
     * @brief pointCoordinateToJoint, 把pointPro转为关节类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    int pointCoordinateToJoint(const PointPro &pointIn, PointPro &jointPointOut
                               ,bool isErrorNotice=true);//没有做停止状态判断，需要调用者判断。
    int pointCoordinateToJoint_noDeviationCheck(const PointPro &pointIn,PointPro &jointPointOut
                                       ,bool isErrorNotice=true);//没有做停止状态判断，需要调用者判断。
    /**
     * @brief pointCoordinateToCartesian, 把pointPro转为笛卡尔类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    int pointCoordinateToCartesian(const PointPro pointIn, PointPro &pointOut);//没有做停止状态判断，需要调用者判断。

    int pointProToFrame(const PointPro& pointPro, Frame &frameOut);

    PointPro frameToPointPro_CART(const Frame& f);

    int printPointPro(PointPro point, QString title);

    float getRunningNumber();
    int setRunningNumber_robotArmControl(float numberIn);
    float getRunningNumber_virtualLineControl();
    int setRunningNumber_virtualLineControl(float numberIn);
    double getBarrierProtectRatioOfMoving();
    int setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn);
    int getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut);
//    int getLineTimeDuration();
//    int getLineRemainingTime();
    PingPongBuffer* getPingPongBuffer();
    int recordDataToWaveBuffer();
    int trajMutexFlag();

    int getFeedbackJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel,
                         QVector<double> &jointAcc, QVector<double> &jointTorque);
    int getAgvSteeringPositionFeedback(QVector<double> &positionOut);
    int setTimeAxisScale(double ratio, double timeDuration);
    int setTimeAxisScale(double ratio, double timeDuration, int timeAxisIndex);
    int setTimeAxisScaleByTrap(double ratio, int timeAxisIndex, int acceleration);
    int setTimeAxisScaleBySpline(double ratio,int timeAxisIndex);

    int getCurrentToolForwardMatrix(Frame &frameReturn);
    int getCurrentToolInverseMatrix(Frame &frameReturn);
    int getCurrentUserForwardMatrix(Frame &frameReturn);
    int getCurrentUserInverseMatrix(Frame &frameReturn);
    int validateZeroData();
    int axisLosePower();
    int axisLosePower(int axisId);
    int calibrateRobotByOffset(std::vector<double> offsetIn);
    int calibrateRobot();//写文件时，占用trajMutex，导致ethercat 超时问题．
    int calibrateRobot(int axisId);
    int calibrateReset();
    int getCalibrateInformation(struct S_AXISGROUP_ZERO_INFORMATION& zeroInformationOut);
    bool isAxisZeroOk();
    bool isAxisZeroOk(int axisId);
    int getAxisZeroState(int axisId);
    QVector<int> getAbsoluteZeroMotorValue();
    QVector<double> getJointMechanicalZeroValue();
    int getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                    QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut );
    int getJointFeedback(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                         QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut );
    bool isAbsoulteMotorBatteryOk();

private:
    int transferRPY(std::vector<double> poseIn,std::vector<double> &poseOut);
    int transRPY2Quan(std::vector<double> poseIn,std::vector<double> &poseOut);
    bool isCoupleControlFollowAxisWork(int followAxisId);
    bool isGantryControlFollowAxisWork(int followAxisId);
    int calculateCurrentCommandPoint_byJointCommand(Frame &positionFrameCommandOut);
    int calibrateCheck();
    bool isLineFinished(double currentLineTimeDurationIn);
    int setServoOnStatus(const QVector<bool> &servoOnStatus);
    int axisFeedbackCheck();
    int axisPositionCommandLimitApproachCheck();
    int laserLoseRescueLogic();//雷达数据丢失补救逻辑
    int readVehicleType(QString filePath,E_VEHICLE_TYPE &typeOut);
    int rosCommunicate();//发布encoder odom, 获取ｒｏｓ position ,获取　ｖｉｓｉｏｎ match position
    int vehicleEncoderOdomUpdate();
    int vehicleJogUpdate();
    int zeroManagementLoopRealTime();
    int zeroManagementLoopRealTime_ethercat();
    int zeroManagementLoopRealTime_inner();
    int zeroManagementLoopCommonTime();
    int vehicleJogLoopCommonTime();
    int vehicleJogLoopRealTime();
    int vehicleRosControlLoopCommonTime();
    int vehicleRosControlLoopRealTime();
    int magneticTrackLoopCommonTime();
    int magneticTrackLoopRealTime();
    int virtualLineTrackLoopCommonTime();
    int virtualLineTrackLoopRealTime();
    int updateVirtualLinePositionFeedback(double velocity_x_in=0);
    int processJointInstruction(MoveParameter moveParameter, E_VELOCITY_TYPE velocityType);
    int processSingleAxisInstruction(MoveParameter moveParameter, E_VELOCITY_TYPE velocityType);
    int processLineInstruction(MoveParameter moveParameter);
    int processCircleInstruction(MoveParameter moveParameter1,MoveParameter moveParameter2);
    int processJointContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove);
    int processCartesianContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove);

    int getCommandQueueSize();
    int getCommandFromQueue(MoveParameter & moveParameter);
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, int parameter1=0, int parameter2=0, int parameter3=0, int parameter4=0);
    int printInformation(QVector<MoveParameter> moveParameterArry, QString title);
    int printInformation(MoveParameter moveParameter, QString title);

    /**
     * @brief setTimeAxisScale　设置时间轴缩放比例
     * @param ratio　缩放比例０～１
     * @param timeDuration　多长时间到达设定比例
     * @return
     */

    int setTimeAxisScaleByTime(double timeDuration);

    int setTimeAxisScale(double ratio);

    int setTimeAxisScale();

    bool isInFineMoving();//是否为最后一条正在运行的轨迹.不加锁

    void* threadRun(void *);
    static void* threadTask( void* classPtr );
    void createThread();

    int cameraScanTrigLoop();//扫描相机触发逻辑




private:
    BeltTrackManager *beltTrackManager;
    UserInfo userInfo;
    DragControl * dragControl;
    MotionMessage * motionMessage;//need todo motionMessage is NULL
    CoordinateManager* coordinateManager;
    ZeroCalculate* zeroCalculate;
    QQueue<MoveParameter> commandQueue;
    AxisGroup* axisGroup;
    PingPongBuffer* pingPongBuffer;//示波器数据采集用
    double timeStamp;//示波器数据采集用

    double totalSpeedRatio;//safeSpeedRatio*controlSpeedRatio*pidSpeedRatio
    double safeSpeedRatio;//外部安全用调速
    double controlSpeedRatio;//手动速率，自动速率用。
    double pidSpeedRatio;//pid调节用。
    double adjustSpeedRatio;//

    double timeAxisScale;//时间轴比例尺，内部求点时，需要修正v=vOLD/timeAxisScale, a=aOLD/timeAxisScale^2 .
    double axisAccelerationRatio;//加速度比例
    double deltaTime;//单步长,unit is s
    MC_TRAJECTORY_STATUS basicStatus;//基本状态
    int jointsNum;//所有轴数
    int innerJointsNum;
    int externJointsNum;//不再使用该参数全部合并为内部轴
    int agvAxisNum;
    int virtualAxisNum;
    int robotId;
    int robotType;
    /**
     * @brief t1RatioLimit T1限速值
     */
//    double t1RatioLimit;

    float targetWaitTime; // unit: s
    float lineMaxSpeed;// unit: mm/s
    float lineMaxAcceleration;// unit: mm/s

    RobotCommonParameter * roboParameter;
    QVector<E_AXIS_LOGIC_TYPE> axisLogicalTypeVector;//使能判断用

    /**
     * @brief userCoordinateFile
     */
    QString userCoordinateFile;
    /**
     * @brief toolCoordinateFile
     */
    QString toolCoordinateFile;


    /**
     * @brief trajMutex mcControllerInterpreter 用于轨迹
     * cammandQueue VelocityProfile ctraject保护, 保护的粒度越小越好，防止死锁。
     */
    pthread_mutex_t trajMutex;
    /**
     * @brief trajAttr 锁参数
     */
    pthread_mutexattr_t trajAttr;

    /**
     * @brief getMutex　用于共用算法数据
     *
     * currentJointPoint_Feedback_old currentCoordinatePoint_Feedback_old 保护
     *
     * ecat的数据采用无锁原子读取操作
     */
    pthread_mutex_t getMutex;
    /**
     * @brief getAttr
     */
    pthread_mutexattr_t getAttr;

    TimeAxisParameter timeAxisParameter;
    int timeAxisStatus;
    double oldTimeAxisRatio;
    double newTimeAxisRatio;

//    PointPro currentJointPoint_Command;notused
//    PointPro currentCoordinatePoint_Command;notused
    PointPro currentJointPoint_Feedback;
    PointPro currentCoordinatePoint_Feedback;
//    QVector<Frame> currentFrame_Feedback;notused
//    PointPro currentJointPoint_FeedbackOld;
//    PointPro currentCoordinatePoint_FeedbackOld;

    QQueue<int> lineQueue;
    QQueue<double> timeDurationQueue;
    //除了stop请他情况不要复位行号
    volatile float currentLine; //0.2=算法已取点, 0.5=正在运行，0.9=(已经到目标位置),  ,下一行+0.2(避免精度丢失,不用0.1) not used
    bool isCurrentLineFinished;
    double currentLineRemainingTime;
    double currentLineTimeDuration;
    double lineTimeCount;
    double lineTimeCountOld;
    int exceedTimeCount;
    MC_MOTION_MODEL motionModel;
    bool isTrajectoryContiousRun;
    volatile bool allowSingleAxisJogFlag;

    MagneticTrack* magneticTrack;
    VehicleJogControl* vehicleJogControl;
    VirtualLineControl* virtualLineControl;
    VehiclePositionManage* vehiclePositionManage;
    Cu1SensorAdapter *cu1Sensor;
    QrcodeSensorAdapter *qrcodeSensorAdapter;
    SensorInfo sensorInfo;
    int maganeticInputIndex;
    volatile bool isMoveToPoint;
    MotionDebugInfo motionDebugInfo;
    pthread_t threadId;

    int allowServonCheck;
    double allowServonCheckDifference;

    HomeFinishCommand homeFinishedCommand;
//    VehiclePosition vehiclePositionFeedback_ros;
//    VehiclePosition vehiclePositionFeedback_visionMatch;

    bool isInitialOkStatus;
    RosApi *m_rosApi;
    E_VEHICLE_TYPE vehicleType;
    int laserLoseRescueFlag;//雷达补救状态。
    int debugFlag;
    RegData *regData;
    CoupleControlManagement *coupleManagement;
    GantryControl *gantryControl;
    int updateTrajStage;
    bool calibrateCheckFinished;
    RobotIo *ioController;
    bool isMotorCommandValid;
    WaveMotion* waveMotionControl;
    E_CHECK_MODE checkMode;
    int controlType;
    E_VELOCITY_TYPE velocityStype_trajServer;
    bool isCoordinatePlanInitialed;//关节规划切换到笛卡尔规划，需要重新初始化一次起始点

    //扫描相机触发相关参数
    int motionTrigLoopCount;
    QQueue<PointPro> motionTrigPose;
    int motionTrigDoIndex;
    double motionTrigPeriod;
    bool motionTrigStart;
    int trigTimeCount;
    int trigStage;

    int slowdownTickcount;
    double originCameraPoint_x;
    double originCameraPoint_y;
    double cameraPointInBase_x;//相机识别点坐标
    double cameraPointInBase_y;
    double cameraPointInBase_z;

    CarvingModule *carvingModule;
    bool isInCarvingTp;
    double tpBeforePose[9];


};

#endif // TRAJECTORYSERVER_H
